#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "proc_utils.h"
void read_childproc(int sig)
{
    int status;
    pid_t id = waitpid(-1, &status, WNOHANG);
    if (WIFEXITED(status))
    {
        printf("Removed proc id: %d \n", id);
        printf("Child send: %d \n", WEXITSTATUS(status));
    }
}
int main() {
    struct sigaction act;
    act.sa_handler = read_childproc;
    sigemptyset(&act.sa_mask);
    act.sa_flags = 0;
    sigaction(SIGCHLD, &act, 0);
    // 1.创建一个通信的socket, 注意第二个参数是：SOCK_DGRAM，数据报的协议。
    int fd = socket(PF_INET, SOCK_DGRAM, 0);
    if(fd == -1)   
        error_handling("socket create error");
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(9999);
    addr.sin_addr.s_addr = INADDR_ANY;	//--服务器绑定本机所有的网卡
    // 2.绑定
    int ret = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
    if(ret == -1) 
        error_handling("bind error");
    // 3.通信
    char recvbuf[512];
    struct sockaddr_in cliaddr;
    socklen_t len = sizeof(cliaddr);	//--用来保存客户端的地址
    // 接收数据
    pid_t vins_pid = 0, px4ctrl_pid = 0, takeoff_pid = 0, planner_pid = 0;
    while (1) {
        memset(recvbuf, 0, sizeof(recvbuf));
        int num = recvfrom(fd, recvbuf, sizeof(recvbuf), 0, (struct sockaddr *)&cliaddr, &len);
        printf("cmd received: %s\n", recvbuf);
        if (strcmp(recvbuf, "vins") == 0 && vins_pid == 0)
        {
            switch (vins_pid = fork()) {
            case -1:
                error_handling("fork error");
            case 0:
                FILE *pip_output;
                // 模拟环境
                //if ((pip_output = popen("roslaunch px4 indoor1.launch 2>&1", "r")) == NULL)
                    //error_handling("popen error");
                // 真机环境
								if ((pip_output = popen("cd /home/nuc2/Fast-Drone-250-master && 123456 | sh shfiles/rspx4.sh 2>&1", "r")) == NULL)
										error_handling("popen error");
                while (1) {
                    if (fgets(recvbuf, 512, pip_output))
                        sendto(fd, recvbuf, strlen(recvbuf) + 1, 0, (struct sockaddr *)&cliaddr, sizeof(cliaddr));
                }
                exit(0);
            default:
                printf("创建了子进程%d\n", vins_pid);
                break;
            }
        } else if (strcmp(recvbuf, "vins_term") == 0 && vins_pid != 0) {
            ProcTree proc_tree;
            proc_tree.kill_descendants(vins_pid);
            vins_pid = 0;
        }
        else if (strcmp(recvbuf, "px4ctrl") == 0 && px4ctrl_pid == 0) {
            switch (px4ctrl_pid = fork()) {
            case -1:
                error_handling("fork error");
            case 0:
                FILE *pip_output;
                // 真机环境
                if ((pip_output = popen("cd /home/nuc2/Fast-Drone-250-master && roslaunch px4ctrl run_ctrl.launch 2>&1", "r")) == NULL)
                    error_handling("popen error");
                // 模拟环境
                //if ((pip_output = popen("cd ~/catkin_ws && bash scripts/xtdrone_run_vio.sh 2>&1", "r")) == NULL)
                 //   error_handling("popen error");
                while (1) {
                    if (fgets(recvbuf, 512, pip_output))
                        sendto(fd, recvbuf, strlen(recvbuf) + 1, 0, (struct sockaddr *)&cliaddr, sizeof(cliaddr));
                }
                exit(0);
            default:
                printf("创建了子进程%d\n", px4ctrl_pid);
                break;
            }
        } else if (strcmp(recvbuf, "px4ctrl_term") == 0 && px4ctrl_pid != 0) {
            ProcTree proc_tree;
            proc_tree.kill_descendants(px4ctrl_pid);
            px4ctrl_pid = 0;
        } else if (strcmp(recvbuf, "takeoff") == 0 && takeoff_pid == 0) {
            switch (takeoff_pid = fork()) {
            case -1:
                error_handling("fork error");
            case 0:
                FILE *pip_output;
                //真机环境
                if ((pip_output = popen("cd /home/nuc2/Fast-Drone-250-master && sh shfiles/takeoff.sh 2>&1", "r")) == NULL)
                     error_handling("popen error");
                // 模拟环境
                //if ((pip_output = popen("cd ~/XTDrone/sensing/slam/vio && python vins_transfer.py iris 0 2>&1", "r")) == NULL)
                 //   error_handling("popen error");
                while (1) {
                    if (fgets(recvbuf, 512, pip_output))
                        sendto(fd, recvbuf, strlen(recvbuf) + 1, 0, (struct sockaddr *)&cliaddr, sizeof(cliaddr));
                }
                exit(0);
            default:
                printf("创建了子进程%d\n", takeoff_pid);
                break;
            }
        } else if (strcmp(recvbuf, "takeoff_term") == 0 && takeoff_pid != 0) {
            ProcTree proc_tree;
            proc_tree.kill_descendants(takeoff_pid);
            takeoff_pid = 0;
        } else if (strcmp(recvbuf, "planner") == 0 && planner_pid == 0) {
            switch (planner_pid = fork()) {
            case -1:
                error_handling("fork error");
            case 0:
                FILE *pip_output;
                // 真机环境
                if ((pip_output = popen("cd /home/nuc2/Fast-Drone-250-master && roslaunch ego_planner single_run_in_exp.launch 2>&1", "r")) == NULL)
                    error_handling("popen error");
                // 模拟环境
                //if ((pip_output = popen("cd ~/XTDrone/communication && python multirotor_communication.py iris 0  2>&1", "r")) == NULL)
                 //   error_handling("popen error");
                while (1) {
                    if (fgets(recvbuf, 512, pip_output))
                        sendto(fd, recvbuf, strlen(recvbuf) + 1, 0, (struct sockaddr *)&cliaddr, sizeof(cliaddr));
                }
                exit(0);
            default:
                printf("创建了子进程%d\n", planner_pid);
                break;
            }
        } else if (strcmp(recvbuf, "planner_term") == 0 && planner_pid != 0) {
            ProcTree proc_tree;
            proc_tree.kill_descendants(planner_pid);
            planner_pid = 0;
        }
    }
    close(fd);
    return 0;
}
